clear all;
close all;

lamb=2;k12=25;k22=25;

Qk=[0;0;0;0];
Zk=[0;0;0;0;0];
Vk=[0;0];

T=0.001;
for k=1:1:10000
    tSpan = [0 T];  
    time(k)=k*T;

    thd=60;         
    th1=Qk(1);dth1=Qk(2);
    th2=Qk(3);dth2=Qk(4);
    Q(:,k)=[th1;th2];

    Oth1=Zk(1);Odth1=Zk(2);
    Oth2=Zk(3);Odth2=Zk(4);Oddth2=Zk(5);
    OQ(:,k)=[Oth1;Oth2];

    dt=0.2*sin(k*T);

    e1=60-Oth1;
    de1=dth1-Odth1;
    e2=60-Oth2;
    de2=dth2-Odth2;
    e(:,k)=[e1;e2];
    de(:,k)=[de1;de2];

    s1=th1+Odth1;
    s2=th2+Odth2;
    s(:,k)=[s1;s2];
    
    [tt2,Vq] = ode45(@(t,x) vv(t,x,s1,s2),tSpan,Vk);
    Vk = Vq(length(Vq),:);

    v1=Vk(1);v2=Vk(2);

    U1=-Odth1-k12*sign(e1)-lamb*abs(s1)^(1/2)*sign(s1)+v1;
    U2=-Odth2-Oddth2-k22*abs(e2)^(1/3)*sign(e2)-lamb*abs(s2)^(1/2)*sign(s2)+v2;
    U(:,k)=[U1;U2];


	[tt,Qq] = ode45(@(t,x) plant(t,x,U1,U2,dt),tSpan,Qk);
    Qk = Qq(length(Qq),:);
	[tt1,Zq] = ode45(@(t,x) obv(t,x,U1,U2,e1,e2),tSpan,Zk);
    Zk = Zq(length(Zq),:);
     
end

figure(1);
plot(time,OQ(1,:),'k',time,OQ(2,:),'r--');
xlabel('time(s)');ylabel('th');
legend('th1','th2');
grid on;

figure(2);
plot(time,U(1,:),'k',time,U(2,:),'r--');
xlabel('time(s)');ylabel('U');
legend('U1','U2');
grid on;

figure(3);
plot(time,e(1,:),'k',time,e(2,:),'r--');
xlabel('time(s)');ylabel('e');
legend('e1','e2');
grid on;